#ifndef matrixes_H
#define matrixes_H

#include <iostream>
#include <map>
#include <vector>
#include <math.h>
#include "geometry.h"

using namespace std;
namespace ABAN
{
    typedef std::vector<FloatType> FloatVector;

    FloatType innerProduct(FloatVector* a,FloatVector* b)
    {
        FloatType result=0.0;
        for(SeqType row = 0; row<a->size(); row++)
            result+=(*a)[row]*(*b)[row];
        return(result);
    }

    union MatrixCoordinate
    {
        uint64_t absCoord;
        struct RowColCoordinate{uint32_t col,row;} rcCoord;
        MatrixCoordinate(uint32_t aRow,uint32_t aCol){rcCoord.row=aRow;rcCoord.col=aCol;}
        MatrixCoordinate(uint64_t idx):absCoord(idx){}
        MatrixCoordinate():absCoord(0){}
        MatrixCoordinate& operator=(const MatrixCoordinate &other){absCoord=other.absCoord;return *this;}
    };
    bool operator< (const MatrixCoordinate &a,const MatrixCoordinate &b){return a.absCoord <  b.absCoord;}
    bool operator==(const MatrixCoordinate &a,const MatrixCoordinate &b){return a.absCoord == b.absCoord;}

    class SparseMatrix
    {
        private:
            SeqType n;

            typedef std::map<MatrixCoordinate,FloatType> SparseStructure;
            typedef SparseStructure::iterator SparseIterator;
            SparseStructure data;
        protected:

        public:
            SparseMatrix(SeqType nn):n(nn){}

            void setVal(SeqType i,SeqType j,FloatType val);
            FloatType getVal(SeqType i,SeqType j);

            SeqType firstRowValIdx(SeqType r);
            SeqType nextRowValIdx(SeqType r,SeqType j);

            inline SeqType getN(){return n;}

            static void matrixDotVec(SparseMatrix* coeffMatrix, FloatVector*ax, FloatVector*x);

            static void SORSolver(SparseMatrix* coeffMatrix, FloatVector* b,
                                  FloatVector* x, FloatVector* x0,
                                  FloatType omega = 1.0, FloatType eps = zeroRadious, SeqType iterMax = 100);
            static void BiCGStabSolver(SparseMatrix* coeffMatrix, FloatVector* b,
                                       FloatVector* x, FloatVector* x0,
                                       FloatType eps = zeroRadious, SeqType iterMax = 150);

    };

    //----------------------------------------------------------------------------
    void SparseMatrix::setVal(SeqType i,SeqType j,FloatType val)
    {
        data[MatrixCoordinate(i,j)] = val;
    }
    //----------------------------------------------------------------------------
    FloatType SparseMatrix::getVal(SeqType i,SeqType j)
    {
        SparseStructure::iterator iter;
        iter = data.find(MatrixCoordinate(i,j));
        if(iter==data.end())
            return 0;
        else
            return iter->second;
    }
    //----------------------------------------------------------------------------
    SeqType SparseMatrix::firstRowValIdx(SeqType r)
    {
        return nextRowValIdx(r,-1);
    }
    //----------------------------------------------------------------------------
    SeqType SparseMatrix::nextRowValIdx(SeqType r,SeqType j)
    {
        SparseIterator iter = data.lower_bound(MatrixCoordinate(r,j+1));
        if (iter==data.end()) return -1;
        else
            if (iter->first.absCoord <= MatrixCoordinate(r,n-1).absCoord)
                return (iter->first.rcCoord.col);
            else
                return -1;
    }
    //----------------------------------------------------------------------------
    void SparseMatrix::matrixDotVec(SparseMatrix* coeffMatrix, FloatVector*x, FloatVector*ax)
    {
        FloatType result;
        SeqType n=coeffMatrix->n;
        SparseIterator traversor=coeffMatrix->data.begin(),endIter=coeffMatrix->data.end();

        for(SeqType row = 0; row<n; row++)
        {
            result = 0.0;

            for (;(traversor!=endIter)&&(traversor->first.rcCoord.row==row);traversor++)
                result+=traversor->second*(*x)[traversor->first.rcCoord.col];

//            for(SeqType col=coeffMatrix->firstRowValIdx(row);
//                col!=-1;
//                col=coeffMatrix->nextRowValIdx(row,col))
//                result+=coeffMatrix->getVal(row,col)*(*x)[col];
             (*ax)[row]=result;
        }

    }
    //----------------------------------------------------------------------------
    void SparseMatrix::SORSolver(SparseMatrix* coeffMatrix, FloatVector* b,
                                 FloatVector* x, FloatVector* x0,
                                 FloatType omega, FloatType eps, SeqType iterMax)
    {
        SeqType iterationCounter=0;
        bool needsNextIteration = true;
        FloatType zigma,err,diagVal;
        FloatVector  delta   (coeffMatrix->getN()),
                     residual(coeffMatrix->getN());

        for (SeqType row = 0; row<coeffMatrix->getN(); row++)
            (*x)[row] = (*x0)[row];

        do
        {
            iterationCounter++;
            /////
            SparseIterator traversor=coeffMatrix->data.begin(),endIter=coeffMatrix->data.end();

            for(SeqType row = 0; row<coeffMatrix->getN(); row++)
            {
                zigma = 0.0;
                diagVal = coeffMatrix->getVal(row,row);

                /////
                for (;(traversor!=endIter)&&(traversor->first.rcCoord.row==row);traversor++)
                    zigma+=traversor->second*(*x)[traversor->first.rcCoord.col];

//                for(SeqType col=coeffMatrix->firstRowValIdx(row);
//                    col!=-1;
//                    col=coeffMatrix->nextRowValIdx(row,col))
//                    zigma+=coeffMatrix->getVal(row,col)*(*x)[col];

                zigma-=diagVal*(*x)[row];
                zigma=((*b)[row]-zigma)/diagVal;
                delta[row]=zigma-(*x)[row];
                (*x)[row]=(*x)[row]+omega*(zigma-(*x)[row]);
            }

            coeffMatrix->matrixDotVec(coeffMatrix,&delta,&residual);
            err=0.0;
            for(SeqType row = 0; row<coeffMatrix->getN() ; row++)
                //L1 norm calculation
                err+=fabs(residual[row]);

            needsNextIteration = ((err/(FloatType)coeffMatrix->getN())>eps);
        }while((iterationCounter<iterMax)&&(needsNextIteration));

        std::cout<<"Iterations = "<<iterationCounter<<"   Error = "<<err/(FloatType)coeffMatrix->getN()<<std::endl;
    }
    //----------------------------------------------------------------------------
    void SparseMatrix::BiCGStabSolver(SparseMatrix* coeffMatrix, FloatVector* b,
                                      FloatVector* x, FloatVector* x0,
                                      FloatType eps, SeqType iterMax)
    {

        SeqType iterCounter=0;
        bool needsNextIteration = true;
        FloatType ro=0.0,roM1,omega,alfa,beta,err;
        FloatVector  ax  (coeffMatrix->getN()),
                     r   (coeffMatrix->getN()),
                     rHat(coeffMatrix->getN()),
                     p   (coeffMatrix->getN()),
                     pHat(coeffMatrix->getN()),
                     s   (coeffMatrix->getN()),
                     sHat(coeffMatrix->getN()),
                     v   (coeffMatrix->getN()),
                     t   (coeffMatrix->getN());


        coeffMatrix->matrixDotVec(coeffMatrix,x,&ax);
        for (SeqType row = 0; row<coeffMatrix->getN(); row++)
            rHat[row] = r[row] = (*b)[row] - ax[row];

        do
        {
            iterCounter++;
            roM1=ro;
            ro=innerProduct(&rHat,&r);

            if (iterCounter==1)
                for (SeqType row = 0; row<coeffMatrix->getN(); row++)
                    p[row] = r[row];
            else
            {
                beta=(ro/roM1)*(alfa/omega);
                for (SeqType row = 0; row<coeffMatrix->getN(); row++)
                    p[row] = r[row] + beta*(p[row]-omega*v[row]);
            }

            for (SeqType row = 0; row<coeffMatrix->getN(); row++)
                pHat[row] = p[row];

            coeffMatrix->matrixDotVec(coeffMatrix,&pHat,&v);

            alfa=ro/innerProduct(&rHat,&v);

            for (SeqType row = 0; row<coeffMatrix->getN(); row++)
                s[row] = r[row] - alfa*v[row];

            for (SeqType row = 0; row<coeffMatrix->getN(); row++)
                sHat[row] = s[row];

            coeffMatrix->matrixDotVec(coeffMatrix,&sHat,&t);

            omega=innerProduct(&t,&s)/innerProduct(&t,&t);

            for (SeqType row = 0; row<coeffMatrix->getN(); row++)
                (*x)[row] = (*x)[row] + alfa*pHat[row] + omega*sHat[row];

            err=0.0;
            for (SeqType row = 0; row<coeffMatrix->getN(); row++)
            {
                r[row] = s[row] - omega*t[row];
                err+=fabs(r[row]);
            }
            needsNextIteration = ((err/(FloatType)coeffMatrix->getN())>eps);
        }while((iterCounter<iterMax)&&(needsNextIteration));
        std::cout<<"Iterations = "<<iterCounter<<"   Error = "<<err/(FloatType)coeffMatrix->getN()<<std::endl;
    }
    //----------------------------------------------------------------------------
}
#endif
